#ifndef NAVIGATION_H
#define NAVIGATION_H

#include <stdint.h>
#include "kalman.h"

#ifdef __cplusplus
extern "C" {
#endif

///////////////// sensor alignment /////////
// mounted as follows
	// travel /\   //
	//		  ||
	//
	//    -x
	// -y     y
	//     x
	// 
	// compass
	//     N
	//	W	  E
	//     S
	
// extract components
#define extract_x_comp(orien, magnitude) (cos(orien)*magnitude)
#define extract_y_comp(orien, magnitude) (sin(orien)*magnitude)
	
#define extract_x(orien) (cos(orien))
#define extract_y(orien) (sin(orien))
	
///////////// accelerometer /////////////
void convert_accelerometer(kal_t orientation, kal_t *measured, 
							   kal_t *xy);
	
///////////////// gps ///////////////////
// 1 minute latitude 1853m
// 1 minute longitude 1418m
#define LAT100K_TO_METER 0.01853
#define LON100K_TO_METER 0.01418

// take two lat or lon points in 10K minute format
// and return difference in meters
#define diff_lat_meters(a,b) (((kal_t)(a-b))*LAT100K_TO_METER) 
#define diff_lon_meters(a,b) (((kal_t)(a-b))*LON100K_TO_METER)
	
// take difference of lat or lon and convert to meters
#define lat_to_meters(a) (((kal_t)a)*LAT100K_TO_METER)
#define lon_to_meters(a) (((kal_t)a)*LON100K_TO_METER)
	
	
#define GPRMC_TERM   "GPRMC"
#define GPGGA_TERM   "GPGGA"

#define GPS_BAD 9999999

enum gps_term_t {GPS_SENTENCE_GPGGA, GPS_SENTENCE_GPRMC, GPS_SENTENCE_OTHER};

struct gps_str {
	// properties
    long latitude, new_latitude;
    long longitude, new_longitude;
	float course, new_course;
	float speed, new_speed;
	
    unsigned long last_position_fix, new_position_fix;
	
	uint32_t id;
	uint8_t haveLock;
	
    // parsing state variables
    uint8_t parity;
    uint8_t is_checksum_term;
	// termSize must be set to the size of term in gps_init.
	uint8_t termSize;
    char term[15];
    uint8_t sentence_type;
    uint8_t term_number;
    uint8_t term_offset;
    uint8_t gps_data_good;
};

typedef struct gps_str gps_t;

void gps_init(gps_t *gps);
uint8_t gps_encode(char c, gps_t *gps);
uint8_t term_complete(gps_t *gps);
int from_hex(char a);
unsigned long parse_degrees(gps_t *gps);
long gpsatol(const char *str);
int gpsstrcmp(const char *str1, const char *str2);
uint8_t gpsisdigit(char c);
unsigned long parse_minutes(gps_t *gps);
float parse_radians(char *term);
float parse_speed(char *term);
float parse_decimal(char *term);
unsigned long get_curr_time();

#ifdef __cplusplus
}
#endif

#endif
